/* file name=sg90.c */
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>

#include <wiringPi.h>
#include <softPwm.h>

#define RANGE 200     /* 1 means 100 us , 200 means 20 ms 1等于100微妙，200等于20毫秒 */

int move( int pin, int moveAngle )
{
	float degree;
	float angle=(float)moveAngle;
	degree = 5 +  angle / 180.0 * 20.0;
	printf("角度=%f\n",degree);

		softPwmWrite(pin,degree);
        delay(200);
		softPwmWrite(pin,degree);
        // delay(200);
		// softPwmWrite(pin,degree);
		//  softPwmStop(pin);
         softPwmWrite(pin,0);
		 delay(40);

        //  softPwmWrite(pin,0);
        //  softPwmWrite(pin,0);
        //  softPwmWrite(pin,0);

	// printf("%d\n",degree);
	// printf("%d\n",moveAngle);
	// // softPwmStop(pin);
	// // 输入 pwm 后，给 pwm 为 0，防抖
	// softPwmWrite( pin, 0 );
	// softPwmWrite( pin, 0 );

    return 0;
}

int fmain( int num,int moveAngle )
{
	// int	num;
	// wpi 引脚 
	int	pinN_1	= 7;
	// int	pinN_2	= 0;
	int	pinN_2	= 0;
	// pinMode(pinN_2,OUTPUT);
	// int	moveAngle_1	= 90;
	// int	moveAngle_2	= 90;
	wiringPiSetup();                        /* wiringpi初始化 */
	softPwmCreate( pinN_1, 15, RANGE );     /* 创建一个使舵机转到90的pwm输出信号 */
	// delay(140);
	softPwmWrite( pinN_1, 0 );
	softPwmCreate( pinN_2, 15, RANGE );     /* 创建一个使舵机转到90的pwm输出信号 */
	// delay(140);
	softPwmWrite( pinN_2, 0 );


		if ( !(( moveAngle ) >= 10 && ( moveAngle ) <= 170) ){
			printf( "degree is between 0 and 180\n" );
		}

		if ( num == 0 || num == 1 ){
			if ( num == 0 ){
				moveAngle = moveAngle - 5;
			}
			if ( num == 1 ){
				moveAngle = moveAngle + 5;
			}
			move( pinN_1, moveAngle );
		}

		if ( num == 2 || num == 3 ){
			if ( num == 2 ){
				moveAngle = moveAngle - 3;
			}
			if ( num == 3 ){
				moveAngle = moveAngle + 3;
			}
			move( pinN_2, moveAngle );
		}
    return moveAngle;
}
// gcc -Wall -o duoji duoji.c -lwiringPi -lwiringPiDev -lpthread -lrt -lm -lcrypt -shared

